#include "WPILib.h"

/**
 * This is a demo program showing the use of the RobotBase class.
 * The SimpleRobot class is the base of a robot application that will automatically call your
 * Autonomous and OperatorControl methods at the right time as controlled by the switches on
 * the driver station or the field controls.
 */
class DefaultRobot : public SimpleRobot
{
	RobotDrive *myRobot;			// robot drive system
	DigitalInput *armUpperLimit;	// arm upper limit switch
	DigitalInput *armLowerLimit;	// arm lower limit switch
	Joystick *rightStick;			// joystick 1 (arcade stick or right tank stick)
	Joystick *leftStick;			// joystick 2 (tank left stick)
	Joystick *armStick;				// joystick 3 to control arm
	DriverStation *ds;				// driver station object
	DigitalInput *pneumatic;		// locking for the solenoid
	Compressor *KarlKompressor;     // this is a compressor
	Solenoid *leftSol;				// this is the left solenoid
	Solenoid *rightSol;				// this is the right solenoid

	enum							// Driver Station jumpers to control program operation
	{ ARCADE_MODE = 1,				// Tank/Arcade jumper is on DS Input 1 (Jumper present is arcade)
	  ENABLE_AUTONOMOUS = 2,		// Autonomous/Teleop jumper is on DS Input 2 (Jumper present is autonomous)
	} jumpers;	                            

public:
	/**
	 * 
	 * 
	 * Constructor for this robot subclass.
	 * Create an instance of a RobotDrive with left and right motors plugged into PWM
	 * ports 1 and 2 on the first digital module.
	 */
	DefaultRobot(void)
	{
		ds = DriverStation::GetInstance();
		myRobot = new RobotDrive(1, 3, 2, 4);	// create robot drive base
		rightStick = new Joystick(1);			// create the joysticks
		leftStick = new Joystick(2);
		pneumatic = new DigitalInput(4,2);		// holds the solenoid in place
		KarlKompressor = new Compressor(4, 1, 4, 1); 
		leftSol = new Solenoid(8,2);
		rightSol = new Solenoid(8,3);
		//Update the motors at least every 100ms.
		GetWatchdog().SetExpiration(0.1);
	}

	/**
	 * Drive left & right motors for 2 seconds, enabled by a jumper (jumper
	 * must be in for autonomous to operate).
	 */
	void Autonomous(void)
	{
		GetWatchdog().SetEnabled(false);
	}
		
		/**
		 * Runs the motors under driver control with either tank or arcade steering selected
		 * by a jumper in DS Digin 0. Also an arm will operate based on a joystick Y-axis. 
		 */
	void OperatorControl(void)
	{
	
		KarlKompressor->Start();
		rightSol->Set(1);
		leftSol->Set(0);
		while (IsOperatorControl())
		{
			if (rightStick->GetTrigger())
			{
				while (pneumatic->GetChannel() == 0)
				{
					rightSol->Set(0);
					leftSol->Set(1);
				}
				rightSol->Set(1);
				leftSol->Set(1);
			}
			
		}
	}
};